#include "region_server_core.hpp"

RegionServer::RegionServer(ros::NodeHandle *nodehandle) : nh_(*nodehandle)
{
    ROS_INFO("Initializing region server...");

    // register ros services
    get_region_service_ = nh_.advertiseService("get_region_service",
                                               &RegionServer::getRegionServiceCallback,
                                               this);
    get_village_service_ = nh_.advertiseService("get_village_service",
                                                &RegionServer::getVillageServiceCallback,
                                                this);
    set_village_service_ = nh_.advertiseService("set_village_service",
                                                &RegionServer::setVillageServiceCallback,
                                                this);
    add_village_service_ = nh_.advertiseService("add_village_service",
                                                &RegionServer::addVillageServiceCallback,
                                                this);
    del_village_service_ = nh_.advertiseService("del_village_service",
                                                &RegionServer::delVillageServiceCallback,
                                                this);

    // load ros param from launch file (ros param server)
    std::string config_file_path;
    nh_.param<std::string>("config_file_path", config_file_path, "./config/villages.json");

    ROS_INFO_STREAM("Config file path: " << config_file_path);
    if(config_file_path.substr(config_file_path.find_last_of(".") + 1) == "json")
    {
        // handle json file
        region_.load_json(config_file_path);
    }
    else
    {
        // handle yaml file
        region_.load_yaml(config_file_path);
    }

    ROS_INFO("region server initialized.");
}

bool RegionServer::getRegionServiceCallback(
    village_test::GetRegion::Request &req,
    village_test::GetRegion::Response &res)
{
    region_msg rm;

    vector<Village> v_list = region_.get_region();
    for (auto it = v_list.begin(); it != v_list.end(); ++it)
    {
        village_msg vm;
        vm.id = it->get_id();
        vm.x = it->get_x();
        vm.y = it->get_y();
        rm.villages.push_back(vm);
    }

    res.region = rm;
    ROS_INFO_STREAM(rm);

    return true;
}

bool RegionServer::getVillageServiceCallback(
    village_test::GetVillage::Request &req,
    village_test::GetVillage::Response &res)
{
    Village *pVillage = region_.get_village(req.id);

    village_msg vm;
    if (pVillage != nullptr)
    {

        vm.id = pVillage->get_id();
        vm.x = pVillage->get_x();
        vm.y = pVillage->get_y();

        res.village = vm;
        ROS_INFO_STREAM("Village (id==" << req.id << ") found:\n"
                                        << vm);

        return true;
    }
    else
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") NOT found.\n");
        return false;
    }
}

bool RegionServer::setVillageServiceCallback(
    village_test::SetVillage::Request &req,
    village_test::SetVillage::Response &res)
{
    res.success = region_.set_village(req.id, req.x, req.y);

    if (res.success)
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") location set to: ( " << req.x << ", " << req.y << " ).\n");
    }
    else
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") location set FAILED.\n");
    }

    return res.success;
}

bool RegionServer::addVillageServiceCallback(
    village_test::AddVillage::Request &req,
    village_test::AddVillage::Response &res)
{
    res.success = region_.add_village(req.id, req.x, req.y);

    if (res.success)
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ", location==( " << req.x << " ," << req.y << " )) is added to the region.\n");
    }
    else
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") add FAILED.\n");
    }

    return res.success;
}

bool RegionServer::delVillageServiceCallback(
    village_test::DelVillage::Request &req,
    village_test::DelVillage::Response &res)
{
    res.success = region_.del_village(req.id);

    if (res.success)
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") deleted from the region.\n");
    }
    else
    {
        ROS_INFO_STREAM("Village (id==" << req.id << ") delete FAILED.\n");
    }

    return res.success;
}